//**************Compass*********************************************
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Compass.h> // Compass Library
#include <AP_Math.h>	// ArduPilot Mega Vector/Matrix math Library
#include <Wire.h>

#define ToDeg(x) (x*57.2957795131)  // *180/pi
#define ToRad(x) (x*0.01745329252)  // *pi/180

FastSerialPort0(Serial);

AP_Compass_HMC5843 compass;
//******************************************************************
//**************RangeFinder*****************************************
#include <AP_RangeFinder.h>     // Range finder library
#include <AP_ADC.h>		// ArduPilot Mega Analog to Digital Converter Library

#define RF_PIN AP_RANGEFINDER_PITOT_TUBE // the pitot tube on the front of the oilpan

AP_ADC_ADS7844	adc; // declare global instances for reading pitot tube


    //AP_RangeFinder_SharpGP2Y aRF; 
AP_RangeFinder_MaxsonarXL aRF; // create the range finder object
//******************************************************************


long timer;
boolean manned;

int wayDegree;

//Constants
#define JET_ZERO 1500

#define AUTOPILOT_SPEED 1550
#define AUTOPILOT_BACKLASH 10
#define AUTOPILOT_STOP_OFFSET 30


#define AUTOPILOT_TURN_HIGH_SPEED_LEFT 1650
#define AUTOPILOT_TURN_HIGH_SPEED_RIGHT 1350

#define AUTOPILOT_TURN_SLOW_SPEED_LEFT 1530
#define AUTOPILOT_TURN_SLOW_SPEED_RIGHT 1470


void setup() {
        //Init Reciever
        RCInit();	// APM Radio initialization
        
        //Init RangeFinder
        adc.Init();            // APM ADC library initialization
        aRF.init(RF_PIN, &adc);

        //Init Compass
        Wire.begin();
        if (!compass.init()) {
	    while (1) ;
        }
        compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD);  // set compass's orientation on aircraft.
        compass.set_offsets(0,0,0);  // set offsets to account for surrounding interference
        compass.set_declination(ToRad(0.0));  // set local difference between magnetic north and true north

        //Fill variables
        wayDegree = 0;
        timer = 0;
        manned = true;

        //Uncomment to debug
        Serial.begin(57600);
        Serial.println("hello!");

        delay(3000);
}

void loop() {
  //Uncomment to debug
  //Serial.print("Sonar: ");
  //Serial.print(aRF.read());
  //print_pwm();	
  
  
   update_mode();
   //Switch low, forward the PPM
   if(manned) //Reading channel 5 to change mode
   {
     //Uncomment to debug
     //Serial.print("manned: ");
      
     OutputCh(0, InputCh(0));
     OutputCh(1, InputCh(1));
     OutputCh(2, InputCh(2));
   }
   else
   {
     //Uncomment to debug
     //Serial.print("unmanned: ");
     int deltaDegree = getDeltaDegree(getCurrentDegree());
     int turnSpeed = JET_ZERO;
     int dirSpeed = JET_ZERO;
     
     if(abs(deltaDegree)<AUTOPILOT_STOP_OFFSET) {
         dirSpeed = AUTOPILOT_SPEED;         
     }
     
     if(deltaDegree>AUTOPILOT_BACKLASH) {
         if(deltaDegree>AUTOPILOT_STOP_OFFSET) {
           turnSpeed = AUTOPILOT_TURN_HIGH_SPEED_LEFT;         
       } else {
         turnSpeed = AUTOPILOT_TURN_SLOW_SPEED_LEFT;
       }
     }
     if((-deltaDegree)>AUTOPILOT_BACKLASH) {
       if((-deltaDegree)>AUTOPILOT_STOP_OFFSET) {
           turnSpeed = AUTOPILOT_TURN_HIGH_SPEED_RIGHT;
       } else {
           turnSpeed = AUTOPILOT_TURN_SLOW_SPEED_RIGHT;
       }
     }
     

     if(aRF.read()<400&&(abs(deltaDegree)<AUTOPILOT_STOP_OFFSET)) {
       turnDirig(); //Поворачиваем
     }

     OutputCh(0, turnSpeed);         
     OutputCh(1, dirSpeed);
      
      //Uncomment to debug
      /*
      Serial.print("Speed:");
      Serial.print(dirSpeed);
      Serial.print(" turn:");
      Serial.print(turnSpeed);
      Serial.print(" delta:");
      Serial.print(deltaDegree);
      Serial.print(" abs:");
      Serial.println(abs(deltaDegree));
      */
     //Выравнивание по координатам
   }
   
  delay(20);
}


void update_mode() {
   if(InputCh(4) < 1500) {
     if(manned){
       manned = false;
       
       wayDegree = getCurrentDegree();

       //Uncomment to debug
       //Serial.print("Way degree: ");
       //Serial.println(wayDegree);
     }
   } else {
      manned = true; 
   }
}

void turnDirig() {
  if(wayDegree<0) {
    wayDegree = wayDegree + 180; 
  } else {
    wayDegree = wayDegree - 180; 
  }
}

int getCurrentDegree() {
  int currentDegree;
  compass.read();
  compass.calculate(0,0);  // roll = 0, pitch = 0 for this example
  currentDegree = ToDeg(compass.heading);
  return currentDegree;
}

int getDeltaDegree(int currentDegree) {
  //currentDegree-wayDegree
  if(currentDegree>0){
     if(wayDegree>(currentDegree-180)) {
       return currentDegree-wayDegree;
     } else {
       return currentDegree-wayDegree-360; 
     }
  } else {
    if(wayDegree<(currentDegree+180)) {
        return currentDegree-wayDegree;       
    } else {
        return currentDegree-wayDegree-360;
    }
  }
}
//*************************************************************
//************** DEBUG ****************************************
//*************************************************************
void print_pwm()
{
  if((millis()- timer) >= 250)
  {
    timer=millis();
    Serial.print(" Ch0:");
    Serial.print(InputCh(0));
    Serial.print(" Ch1:");
    Serial.print(InputCh(1));
    Serial.print(" Ch2:");
    Serial.print(InputCh(2));
    Serial.print(" Ch3:");
    Serial.print(InputCh(3));
    Serial.print(" Ch4:");
    Serial.print(InputCh(4));
    Serial.print(" Ch5:");
    Serial.print(InputCh(5));
    Serial.print(" Ch6:");
    Serial.print(InputCh(6));
    Serial.print(" Ch7:");
    Serial.println(InputCh(7));
  }
}
